#include <stdio.h>
#include <iostream>

#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
// #include <opencv2/nonfree/features2d.hpp>
#include <opencv2/legacy/legacy.hpp>
using namespace cv;
using namespace std;
#include "CameraDetect.hpp"

#define COMPARE_adapt 0
#define COMPARE_otsu 0
#define COMPARE_mix 0
#define COMPARE_traversal 0
#define COMPARE_thresBG 1
#define COMPARE_aruco 0
#define SHOWIMAGE 1

#if SHOWIMAGE==0
#define imshow() 
#endif

#if COMPARE_aruco
#include "aruco\src\aruco.h"
using namespace aruco;
#pragma comment(lib,"aruco/aruco207.lib")
#endif // 

#ifdef TEST
int main(int argc, char** argv)
{
	VideoCapture cap("test.avi");
	Mat markImg, img_adapt, img_otsu, img_mix, img_traversal, img_aruco;
	vector<Marker_J> mark_adapt, mark_otsu, mark_mix, mark_traversal;
#if COMPARE_aruco
	vector< Marker > mark_aruco;
#endif
	double timeMark_adapt = -1, timeMark_otsu = -1, timeMark_mix = -1, timeMark_traversal = -1, timeMark_aruco = -1;
	int count1 = 0, count2 = 0;
#if COMPARE_aruco
	MarkerDetector MDetector;
	CameraParameters CamParam;
	MDetector.setThresholdParams(7, 7);
	MDetector.setThresholdParamRange(2, 0);
#endif
	markDetectInit();
capture:
	if (cap.isOpened())
	{
		cap >> markImg;
	}
	else
	{
		printf("can't open camera");
		return 0;
	}

#ifdef WIN32
	system("cls");
#else 
	system("clear");
#endif

#if COMPARE_adapt
	timeMark_adapt = getTickCount();
	detectCamPos(markImg, mark_adapt, thres_adapt); // thres_traversal thres_otsu
	timeMark_adapt = getTickCount() - timeMark_adapt;
	timeMark_adapt /= (getTickFrequency() / 1000);
#if SHOWIMAGE
	markImg.copyTo(img_adapt);
	for (vector<Marker_J>::iterator it = mark_adapt.begin(); it != mark_adapt.end(); it++)
	{
		it->drawContour(img_adapt);
	}
	imshow("adapt", img_adapt);
#endif // SHOWIMAGE
	cout << "adapt cost: " << (int)timeMark_adapt << "   count: " << mark_adapt.size() << endl;
#endif

#if COMPARE_otsu
	timeMark_otsu = getTickCount();
	detectCamPos(markImg, mark_otsu, thres_adapt);
	timeMark_otsu = getTickCount() - timeMark_otsu;
	timeMark_otsu /= (getTickFrequency() / 1000);
#if SHOWIMAGE
	markImg.copyTo(img_otsu);
	for (vector<Marker_J>::iterator it = mark_otsu.begin(); it != mark_otsu.end(); it++)
	{
		it->drawContour(img_otsu);
	}
	imshow("otsu", img_otsu);
#endif // SHOWIMAGE
	cout << "otsu cost: " << (int)timeMark_otsu << "   count: " << mark_otsu.size() << endl;
#endif

#if COMPARE_mix
	timeMark_mix = getTickCount();
	detectCamPos(markImg, mark_mix, thres_mix);
	timeMark_mix = getTickCount() - timeMark_mix;
	timeMark_mix /= (getTickFrequency() / 1000);
#if SHOWIMAGE
	markImg.copyTo(img_mix);
	for (vector<Marker_J>::iterator it = mark_mix.begin(); it != mark_mix.end(); it++)
	{
		it->drawContour(img_mix);
	}
	imshow("mix", img_mix);
#endif // SHOWIMAGE
	cout << "mix cost: " << (int)timeMark_mix << "   count: " << mark_mix.size() << endl;
#endif

#if COMPARE_traversal
	timeMark_traversal = getTickCount();
	detectCamPos(markImg, mark_traversal, thres_traversal);
	timeMark_traversal = getTickCount() - timeMark_traversal;
	timeMark_traversal /= (getTickFrequency() / 1000);
#if SHOWIMAGE
	markImg.copyTo(img_traversal);
	for (vector<Marker_J>::iterator it = mark_traversal.begin(); it != mark_traversal.end(); it++)
	{
		it->drawContour(img_traversal);
	}
	imshow("traversal", img_traversal);
#endif // SHOWIMAGE
	cout << "traversal cost: " << (int)timeMark_traversal << "   count: " << mark_traversal.size() << endl;
#endif // COMPARE_traversal

#if COMPARE_thresBG
	timeMark_traversal = getTickCount();
	detectCamPos(markImg, mark_traversal, thres_BG);
	timeMark_traversal = getTickCount() - timeMark_traversal;
	timeMark_traversal /= (getTickFrequency() / 1000);
#if SHOWIMAGE
	markImg.copyTo(img_traversal);
	for (vector<Marker_J>::iterator it = mark_traversal.begin(); it != mark_traversal.end(); it++)
	{
		it->drawContour(img_traversal);
		imshow("thresBG", img_traversal);
		waitKey(5000);
	}
	imshow("thresBG", img_traversal);
#endif // SHOWIMAGE
	cout << "thresBG cost: " << (int)timeMark_traversal << "   count: " << mark_traversal.size() << endl;
#endif // COMPARE_traversal

#if COMPARE_aruco
	markImg.copyTo(img_aruco);
	timeMark_aruco = getTickCount();
	mark_aruco = MDetector.detect(markImg, CamParam, -1);
	timeMark_aruco = getTickCount() - timeMark_aruco;
	timeMark_aruco /= (getTickFrequency() / 1000);
#if SHOWIMAGE
	markImg.copyTo(img_aruco);
	for (vector<Marker>::iterator it = mark_aruco.begin(); it != mark_aruco.end(); it++)
	{
		it->draw(img_aruco, Scalar(255, 0, 0));
	}
	imshow("aruco", img_aruco);
#endif // SHOWIMAGE
	cout << "aruco cost: " << (int)timeMark_aruco << "   count: " << mark_aruco.size() << endl;
#endif
	int key;
	if (cap.isOpened())
	{
		key = cvWaitKey(10);
		if (key == 27)
		{
			cout << endl;
			return 0;
		}
		goto capture;
	}
	else
		cvWaitKey();
}

#endif // TEST

